#ifndef  PC_HANDLE
#define PC_HANDLE 
 
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>
#include <sys/shm.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <vector>
#include <string>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <pcl/common/time.h>
#include <pcl/common/angles.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <tf/transform_broadcaster.h>
#include"utils.hpp"
#include"calib.hpp"
using namespace std;
using PointType = pcl::PointXYZI;

class PCHandle{
    public:
        PointType object_center_;
        pcl::PointCloud<PointType>::Ptr global_object_pc_;
        vector<DataRet> global_obj_inf_;
        //deque time;
        deque<tf::StampedTransform> stamp_tf_;
        DataRet local_obj_inf_;
        ObjectPc local_object_pc_;
     
    private:
        float array_3dbbox_[8][3]={{1., 1., -1.}, 
                                                                {1., -1., -1.},
                                                                {-1., -1., -1.},
                                                                {-1., 1., -1.},{1., 1., 1.}, {1., -1., 1.},{-1., -1., 1.}, {-1., 1., 1.}};
        cv::Mat mat_3dbbox_;
        bool is_get_new_object_ = false;
        pcl::PointCloud<PointType>::Ptr pc_plane_for_pca_,pc_x_for_pca_;
        ofstream out_file_;
        mutex tf_lock_, pc_lock_;
        deque<tf::StampedTransform> stamp_tf_temp_;

    public:
        PCHandle(){ 
            local_object_pc_.p_object_pc.reset(new pcl::PointCloud<PointType>);  //need instance first otherwise  has a bug 

            mat_3dbbox_= cv::Mat(8, 3, CV_32F, (float*)array_3dbbox_ );
            time_t t = time(NULL);
            char ch[64] = {0};
            char result[100] = {0};
            strftime(ch, sizeof(ch) - 1, "%Y%m%d--%H%M%S", localtime(&t));
            sprintf(result, "%s", ch);
            string pack_path = ros::package::getPath("mm_perceive");
            string parent_dir =  pack_path + "/data/obj_inf/";
            string time_name = string(result);
            string bar = "/";
            mkdir((parent_dir+time_name+bar).c_str(),S_IRUSR | S_IWUSR | S_IXUSR | S_IRWXG | S_IRWXO);
            string file = "global_object.txt";
            out_file_ = ofstream(parent_dir+time_name+bar+file, std::ios::app);
         }
          
        ~PCHandle() {
        }   

    public:
        // bool get_right_tf(tf::StampedTransform& s_tf, float t_stamp){
        //     float time_min_gap=10000;
        //     float time_gap=0;
        //     int idx_need;
        //     {
        //         lock_guard<mutex> tf_lock(tf_lock_);
        //         for(int i=0;i<stamp_tf_.size();i++){
        //             time_gap = abs(stamp_tf_[i].stamp_.toSec()- t_stamp);
        //             // cout<<"(stamp_tf_[i].stamp_.toNSec() "<<stamp_tf_[i].stamp_.toSec()<<endl;
        //             // cout<<"t_stamp "<<t_stamp<<endl;
        //             // cout<<"gap "<<time_gap<<endl;
        //             // cout<<"tf -  pc  "<<stamp_tf_[i].stamp_.toSec()- t_stamp<<endl;
        //             if(time_gap<time_min_gap){
        //                 time_min_gap = time_gap;
        //                 idx_need = i;
        //             }
        //         }
        //         if(time_min_gap<100){
        //             s_tf = stamp_tf_[idx_need];
        //             return true;
        //         }
        //         else
        //             return false;
        //     }
        // }

    public:
        pcl::PointCloud<PointType>::Ptr get_pc(){
            lock_guard<mutex> pc_lock(pc_lock_);
            return local_object_pc_.p_object_pc;
        }

        DataRet get_local_obj_inf(){
            lock_guard<mutex> pc_lock(pc_lock_);
            return local_obj_inf_;
        }

        vector<DataRet> get_global_inf(){
            lock_guard<mutex> pc_lock(pc_lock_);
            return global_obj_inf_;
        }

        // void get_tf(tf::StampedTransform s_tf){
        //     fixed_size_push_back(stamp_tf_temp_, s_tf,20);
        //     {
        //         lock_guard<mutex> tf_lock(tf_lock_);
        //         stamp_tf_ = stamp_tf_temp_;
        //     }
        // }

        void get_object_pc(ObjectPc p_pc){
            lock_guard<mutex> pc_lock(pc_lock_);
            if(p_pc.p_object_pc->empty() ){
                ROS_ERROR("crop object has no points");
                is_get_new_object_ = false;
                return;
            }
            pcl::PointCloud<PointType>::Ptr pc_temp(new pcl::PointCloud<PointType>);
            try{
                pcl::VoxelGrid<PointType> dw;
                dw.setInputCloud(p_pc.p_object_pc);
                dw.setLeafSize(0.09f,0.09f,0.09f);
                dw.filter(*pc_temp);
            }
            catch (pcl::PCLException ex){
                ROS_ERROR("%s", ex.what());
                ROS_ERROR("can not downsample");
            }
                        // Creating the KdTree object for the search method of the extraction
            pcl::search::KdTree<PointType>::Ptr tree(new pcl::search::KdTree<PointType>);
            tree->setInputCloud (pc_temp); //创建点云索引向量，用于存储实际的点云信息
            std::vector<pcl::PointIndices> cluster_indices;
            pcl::EuclideanClusterExtraction<PointType> ec;
            ec.setMinClusterSize (10);//设置一个聚类需要的最少点数目为100
            ec.setMaxClusterSize (25000);//设置一个聚类需要的最大点数目为25000
            ec.setSearchMethod (tree);//设置点云的搜索机制
            ec.setInputCloud (pc_temp);
            float radius = 0.15;  //if pc is very small, it will have a bug
            // cout<<"bug "<<endl;
            // cout<<"poinsts num "<<pc_temp->points.size()<<endl;
            if(pc_temp->points.size() < 10){
                ROS_ERROR("pointcloud points number less than 10");
                is_get_new_object_ = false;
                return;
            }
            while(cluster_indices.size() ==0 &&  radius < 1.1){
                try{
                    ec.setClusterTolerance (radius); //设置近邻搜索的搜索半径为2cm                       need change
                    ec.extract (cluster_indices);//从点云中提取聚类，并将点云索引保存在cluster_indices中
                    radius = radius + 0.02;
                    // cout<<"cluster num: "<<cluster_indices.size()<<"  radius  "<<radius<<endl;
                }
                catch (pcl::PCLException ex){
                    ROS_ERROR("%s", ex.what());
                    cluster_indices =  std::vector<pcl::PointIndices>();
                    break;
                }
            }
            // cout<<"bug2"<<endl;
            /*为了从点云索引向量中分割出每个聚类，必须迭代访问点云索引，每次创建一个新的点云数据集，并且将所有当前聚类的点写入到点云数据集中。*/
            //迭代访问点云索引cluster_indices，直到分割出所有聚类
            pcl::PointCloud<PointType>::Ptr cloud_get(new pcl::PointCloud<PointType>);
            if(cluster_indices.size() > 0){
                float x_dis = 100;
                Eigen::Vector4f pcaCentroid;
                int j = 0;
                for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){
                    pcl::PointCloud<PointType>::Ptr cloud_cluster(new pcl::PointCloud<PointType>);
                    //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
                    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
                        cloud_cluster->points.push_back (pc_temp->points[*pit]); //*
                    cloud_cluster->width = cloud_cluster->points.size ();
                    cloud_cluster->height = 1;
                    cloud_cluster->is_dense = true;
                    // std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size() << " data points." << std::endl;
                    pcl::compute3DCentroid(*cloud_cluster, pcaCentroid);
                    if (pcaCentroid(0) < x_dis ){
                        x_dis = pcaCentroid(0);
                        cloud_get = cloud_cluster;
                    }
                    j++;
                    if(j>=2){
                        break;
                    }
                }
            }
            if(cloud_get->points.size() ==0 ){
                ROS_ERROR("EuclideanClusterExtraction  get no object points");
                is_get_new_object_ = false;
            }else{
                local_object_pc_.p_object_pc.reset(new pcl::PointCloud<PointType>);  
                pcl::copyPointCloud(*cloud_get, *local_object_pc_.p_object_pc);
                local_object_pc_.time_stamp = p_pc.time_stamp;
                local_object_pc_.idx = p_pc.idx;
                pc_plane_for_pca_.reset(new pcl::PointCloud<PointType>);
                pc_x_for_pca_.reset(new pcl::PointCloud<PointType>);
                for(auto & p:cloud_get->points){
                    p.z = 0;
                    pc_plane_for_pca_->points.push_back(p);
                }
                for(auto & p:cloud_get->points){
                    p.y = 0;
                    pc_x_for_pca_->points.push_back(p);
                }
                is_get_new_object_ = true;
            }
        }

        void get_bbox_save(){
            lock_guard<mutex> pc_lock(pc_lock_);
            if(!is_get_new_object_){
                ROS_ERROR("no object pc, no object pc after cluster.");
                return;
            }
            Eigen::Vector4f pcaCentroid;
            pcl::compute3DCentroid(*local_object_pc_.p_object_pc, pcaCentroid);
            // Eigen::Matrix3f covariance;
            // pcl::computeCovarianceMatrixNormalized(*local_object_pc_.p_object_pc, pcaCentroid, covariance);
            // Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
            // Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
            // Eigen::Vector3f eigenValuesPCA = eigen_solver.eigenvalues();
            // eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); //校正主方向间垂直
            // eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));
            // eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));

            Eigen::Vector4f pcaCentroid_plane;
            pcl::compute3DCentroid(*pc_plane_for_pca_, pcaCentroid_plane);
            Eigen::Matrix3f covariance_plane;
            pcl::computeCovarianceMatrixNormalized(*pc_plane_for_pca_, pcaCentroid_plane, covariance_plane);
            Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver_plane(covariance_plane, Eigen::ComputeEigenvectors);
            Eigen::Matrix3f eigenVectorsPCA_plane = eigen_solver_plane.eigenvectors();
            Eigen::Vector3f eigenValuesPCA_plane = eigen_solver_plane.eigenvalues();
            eigenVectorsPCA_plane.col(2) = eigenVectorsPCA_plane.col(0).cross(eigenVectorsPCA_plane.col(1)); //校正主方向间垂直
            eigenVectorsPCA_plane.col(0) = eigenVectorsPCA_plane.col(1).cross(eigenVectorsPCA_plane.col(2));
            eigenVectorsPCA_plane.col(1) = eigenVectorsPCA_plane.col(2).cross(eigenVectorsPCA_plane.col(0));
            Eigen::Matrix3f eigenVectorsPCA_adj;
      
            eigenVectorsPCA_adj.col(0)  = eigenVectorsPCA_plane.col(2);
            eigenVectorsPCA_adj.col(1)  = eigenVectorsPCA_plane.col(1);
            eigenVectorsPCA_adj.col(2)  = eigenVectorsPCA_plane.col(0);

            // std::cout << "特征值va(3x1):\n" << eigenValuesPCA_plane << std::endl;
            // std::cout << "特征向量ve(3x3):\n" << eigenVectorsPCA_plane << std::endl;
            // std::cout << "质心点(4x1):\n" << pcaCentroid_plane << std::endl;

            Eigen::Matrix4f tm = Eigen::Matrix4f::Identity();
            Eigen::Matrix4f tm_inv = Eigen::Matrix4f::Identity();
            tm.block<3, 3>(0, 0) = eigenVectorsPCA_adj.transpose();   //R.
            tm.block<3, 1>(0, 3) = -1.0f * (eigenVectorsPCA_adj.transpose()) *(pcaCentroid.head<3>());//  -R*t
            tm_inv = tm.inverse();

            // std::cout << "变换矩阵tm(4x4):\n" << tm << std::endl;
            // std::cout << "逆变矩阵tm'(4x4):\n" << tm_inv << std::endl;

            pcl::PointCloud<PointType>::Ptr transformedCloud(new pcl::PointCloud<PointType>);
            pcl::transformPointCloud(*local_object_pc_.p_object_pc, *transformedCloud, tm);

            PointType min_p1, max_p1;
            Eigen::Vector3f c1, c;
            pcl::getMinMax3D(*transformedCloud, min_p1, max_p1);
            c1 = 0.5f*(min_p1.getVector3fMap() + max_p1.getVector3fMap());

            Eigen::Affine3f tm_inv_aff(tm_inv);
            pcl::transformPoint(c1, c, tm_inv_aff);

            // std::cout << "型心c1(3x1):\n" << c << std::endl;

            Eigen::Vector3f whd1;
            whd1 = max_p1.getVector3fMap() - min_p1.getVector3fMap();

            // std::cout << "width1=" << whd1(0) << endl;
            // std::cout << "heght1=" << whd1(1) << endl;
            // std::cout << "depth1=" << whd1(2) << endl;

            cv::Mat corners=cv::Mat::zeros(8,3,CV_32F);
            corners.colRange(0, 1) = mat_3dbbox_.colRange(0, 1) *whd1(0) /2;
            corners.colRange(1, 2) = mat_3dbbox_.colRange(1, 2) *whd1(1)/2;
            corners.colRange(2, 3) = mat_3dbbox_.colRange(2, 3) *whd1(2)/2;
            float T_array[3][3]={tm_inv_aff(0,0),tm_inv_aff(0,1),tm_inv_aff(0,2),
                                                     tm_inv_aff(1,0), tm_inv_aff(1,1), tm_inv_aff(1,2),
                                                     tm_inv_aff(2,0), tm_inv_aff(2,1), tm_inv_aff(2,2) };
            cv::Mat T(3, 3, CV_32F, (float*)T_array );
            corners = corners*T;
            add(c(0),  corners.colRange(0, 1),  corners.colRange(0, 1));
            add(c(1),  corners.colRange(1, 2),  corners.colRange(1, 2));
            add(c(2),  corners.colRange(2, 3),  corners.colRange(2, 3));

            local_obj_inf_.idx = local_object_pc_.idx;
            local_obj_inf_.time_stamp = local_object_pc_.time_stamp;
            local_obj_inf_.center.x = c(0);
            local_obj_inf_.center.y = c(1);
            local_obj_inf_.center.z = c(2);
            local_obj_inf_.lwh.x = whd1(0);
            local_obj_inf_.lwh.y = whd1(1);
            local_obj_inf_.lwh.z = whd1(2);
            local_obj_inf_.bbox_corners = corners;

            //save global inf
            transform_save_pc(local_obj_inf_); 
            //save gloabl inf

            is_get_new_object_ = false;
        }

        void transform_save_pc(DataRet local_inf){   //need handle in order
            tf::StampedTransform right_tf;
            PointType temp_point;
            if(get_tf("/map","/lidar_link", local_inf.time_stamp,right_tf)){
                ROS_INFO("get right tf");
                DataRet global_inf;
                global_inf.time_stamp = local_inf.time_stamp;
                global_inf.idx = local_inf.idx;
                global_inf.lwh = local_inf.lwh;
                pcl::PointCloud<PointType>::Ptr point_trans(new pcl::PointCloud<PointType>);
                point_trans->points.push_back(local_inf.center);
                for(int i = 0; i < local_inf.bbox_corners.size().height;i++){
                    temp_point.x = local_inf.bbox_corners.at<float>(i, 0);
                    temp_point.y = local_inf.bbox_corners.at<float>(i, 1);
                    temp_point.z = local_inf.bbox_corners.at<float>(i, 2);
                    point_trans->points.push_back(temp_point);
                }
                Eigen::Matrix4f T_local2map = Eigen::Matrix4f::Identity();
                Eigen::Quaternionf q_odom(right_tf.getRotation().w(),right_tf.getRotation().x(),
                            right_tf.getRotation().y(),right_tf.getRotation().z());
                T_local2map.block<3, 3>(0, 0) = q_odom.toRotationMatrix();
                T_local2map.block<3, 1>(0, 3) = Eigen::Vector3f(right_tf.getOrigin().x(), right_tf.getOrigin().y(), right_tf.getOrigin().z());
                // cout<<"tf "<<right_tf.getOrigin().x()<<"y "<<right_tf.getOrigin().y()<<"z "<<right_tf.getOrigin().z()<<endl;
                pcl::transformPointCloud(*point_trans, *point_trans, T_local2map);
                global_inf.center =point_trans->points[0];
                global_inf.bbox_corners = cv::Mat(8, 3, CV_32F);
                for(int i = 1; i < local_inf.bbox_corners.size().height +1;i++){
                    global_inf.bbox_corners.at<float>(i-1, 0) = point_trans->points[i].x;
                    global_inf.bbox_corners.at<float>(i-1, 1) = point_trans->points[i].y;
                    global_inf.bbox_corners.at<float>(i-1, 2) = point_trans->points[i].z;
                }
                global_obj_inf_.push_back(global_inf);
                // save inf need global in txt
                out_file_<<global_inf.idx<<" "<<
                to_string(global_inf.center.x)<<" "<<to_string(global_inf.center.y)<<" "<<to_string(global_inf.center.z)<<" "<<
                to_string(global_inf.lwh.x)<<" "<<to_string(global_inf.lwh.y)<<" "<<to_string(global_inf.lwh.z)<<" "<<endl;
                // save inf need in txt
                ROS_INFO("save gloabal object info ok!");
            }else{
                ROS_ERROR("save nothing, have no right tf. wait for a moment or maybe tf rate has a bug ");
            }
        }
};

// class PcDataLoader{
//     public:
        
// }
#endif
